#!/usr/bin/python3

import roslib
import rospy
import math
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped
import time
import numpy as np
from std_msgs.msg import Float64MultiArray
from sensor_msgs.msg import JointState
import copy
import sys
import roslibpy

class twining_bridge():
    def __init__(self,nodename):
        pass

    def establish_key_components(self, key_components):

        for i in key_components.keys():
            trans = TransformStamped()

            trans.header.frame_id = "world" 
            trans.header.stamp = rospy.Time.now()
            trans.child_frame_id = i + "/base_link"
            #rospy.loginfo(trans.child_frame_id)

            trans.transform.translation.x = key_components[i][0]
            trans.transform.translation.y = key_components[i][1]
            trans.transform.translation.z = key_components[i][2]

            rotation = tf.transformations.quaternion_from_euler(key_components[i][3], key_components[i][4], key_components[i][5])
            trans.transform.rotation.x = rotation[0]
            trans.transform.rotation.y = rotation[1]
            trans.transform.rotation.z = rotation[2]
            trans.transform.rotation.w = rotation[3]

            self.appendTransform(trans)

        self.sendStaticTransforms()

    def appendTransform(self,trans):
        for i in range(len(self.staticTransformList)):
            if(self.staticTransformList[i].header.frame_id==trans.header.frame_id and 
                self.staticTransformList[i].child_frame_id == trans.child_frame_id):
                print("already exist",self.staticTransformList[i])
                self.staticTransformList[i]=trans
                print("already exist",self.staticTransformList[i])
                return 
        self.staticTransformList.append(trans)
        return 

    def sendStaticTransforms(self):
        if(len(self.staticTransformList)>0):
            self.tfBroadcaster.sendTransform(self.staticTransformList)
        return

    def getValidRosServer(self,ip_addr):
        if self.rosServer is None:
            self.rosServer = roslibpy.Ros(host = ip_addr, port=9090)
            self.rosServer.run()
        return self.rosServer
    
    def close(self):
        if self.rosServer is not None:
            self.rosServer.terminate()
            self.rosServer = None

    def republish_by_roslibpy(self,msg,pub):
        # if(not self.realtime_tracking_switch_on):
        #     return 

        # if(self.joint_name_index is None):
        #     para_name = "/pogo_group/ptp_controller/joints"
        #     if(rospy.has_param(para_name)):
        #         self.joint_names = rospy.get_param(para_name)
        #         self.joint_name_index = {self.joint_names[i]:i for i in range(len(self.joint_names))}
        #     else:
        #         return

        # sent_command=[0.0]*len(self.joint_names)
        # for i in range(len(msg.name)):
        #     sent_command[self.joint_name_index[msg.name[i]]] = msg.position[i]
        sent_command=roslibpy.Message({
            'header':roslibpy.Header(0,roslibpy.Time(0,0),''),
            'name':msg.name,
            'position':msg.position
        })

        pub.publish(sent_command)
        return

    # def turnOnRealtimeTracking_cb(self,msg):
    #     print(msg)
    #     return

    def run(self):
        rospy.spin()

if __name__ == '__main__':

    r = twining_bridge('rotors2_twining_bridge')
    r.run()
